namespace Eigen {

namespace internal {

    template <typename Scalar>
    void lmpar(Matrix<Scalar, Dynamic, Dynamic>& r,
               const VectorXi& ipvt,
               const Matrix<Scalar, Dynamic, 1>& diag,
               const Matrix<Scalar, Dynamic, 1>& qtb,
               Scalar delta,
               Scalar& par,
               Matrix<Scalar, Dynamic, 1>& x)
    {
        using std::abs;
        using std::sqrt;
        typedef DenseIndex Index;

        /* Local variables */
        Index i, j, l;
        Scalar fp;
        Scalar parc, parl;
        Index iter;
        Scalar temp, paru;
        Scalar gnorm;
        Scalar dxnorm;

        /* Function Body */
        const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
        const Index n = r.cols();
        eigen_assert(n == diag.size());
        eigen_assert(n == qtb.size());
        eigen_assert(n == x.size());

        Matrix<Scalar, Dynamic, 1> wa1, wa2;

        /* compute and store in x the gauss-newton direction. if the */
        /* jacobian is rank-deficient, obtain a least squares solution. */
        Index nsing = n - 1;
        wa1 = qtb;
        for (j = 0; j < n; ++j)
        {
            if (r(j, j) == 0. && nsing == n - 1)
                nsing = j - 1;
            if (nsing < n - 1)
                wa1[j] = 0.;
        }
        for (j = nsing; j >= 0; --j)
        {
            wa1[j] /= r(j, j);
            temp = wa1[j];
            for (i = 0; i < j; ++i) wa1[i] -= r(i, j) * temp;
        }

        for (j = 0; j < n; ++j) x[ipvt[j]] = wa1[j];

        /* initialize the iteration counter. */
        /* evaluate the function at the origin, and test */
        /* for acceptance of the gauss-newton direction. */
        iter = 0;
        wa2 = diag.cwiseProduct(x);
        dxnorm = wa2.blueNorm();
        fp = dxnorm - delta;
        if (fp <= Scalar(0.1) * delta)
        {
            par = 0;
            return;
        }

        /* if the jacobian is not rank deficient, the newton */
        /* step provides a lower bound, parl, for the zero of */
        /* the function. otherwise set this bound to zero. */
        parl = 0.;
        if (nsing >= n - 1)
        {
            for (j = 0; j < n; ++j)
            {
                l = ipvt[j];
                wa1[j] = diag[l] * (wa2[l] / dxnorm);
            }
            // it's actually a triangularView.solveInplace(), though in a weird
            // way:
            for (j = 0; j < n; ++j)
            {
                Scalar sum = 0.;
                for (i = 0; i < j; ++i) sum += r(i, j) * wa1[i];
                wa1[j] = (wa1[j] - sum) / r(j, j);
            }
            temp = wa1.blueNorm();
            parl = fp / delta / temp / temp;
        }

        /* calculate an upper bound, paru, for the zero of the function. */
        for (j = 0; j < n; ++j) wa1[j] = r.col(j).head(j + 1).dot(qtb.head(j + 1)) / diag[ipvt[j]];

        gnorm = wa1.stableNorm();
        paru = gnorm / delta;
        if (paru == 0.)
            paru = dwarf / (std::min)(delta, Scalar(0.1));

        /* if the input par lies outside of the interval (parl,paru), */
        /* set par to the closer endpoint. */
        par = (std::max)(par, parl);
        par = (std::min)(par, paru);
        if (par == 0.)
            par = gnorm / dxnorm;

        /* beginning of an iteration. */
        while (true)
        {
            ++iter;

            /* evaluate the function at the current value of par. */
            if (par == 0.)
                par = (std::max)(dwarf, Scalar(.001) * paru); /* Computing MAX */
            wa1 = sqrt(par) * diag;

            Matrix<Scalar, Dynamic, 1> sdiag(n);
            qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);

            wa2 = diag.cwiseProduct(x);
            dxnorm = wa2.blueNorm();
            temp = fp;
            fp = dxnorm - delta;

            /* if the function is small enough, accept the current value */
            /* of par. also test for the exceptional cases where parl */
            /* is zero or the number of iterations has reached 10. */
            if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
                break;

            /* compute the newton correction. */
            for (j = 0; j < n; ++j)
            {
                l = ipvt[j];
                wa1[j] = diag[l] * (wa2[l] / dxnorm);
            }
            for (j = 0; j < n; ++j)
            {
                wa1[j] /= sdiag[j];
                temp = wa1[j];
                for (i = j + 1; i < n; ++i) wa1[i] -= r(i, j) * temp;
            }
            temp = wa1.blueNorm();
            parc = fp / delta / temp / temp;

            /* depending on the sign of the function, update parl or paru. */
            if (fp > 0.)
                parl = (std::max)(parl, par);
            if (fp < 0.)
                paru = (std::min)(paru, par);

            /* compute an improved estimate for par. */
            /* Computing MAX */
            par = (std::max)(parl, par + parc);

            /* end of an iteration. */
        }

        /* termination. */
        if (iter == 0)
            par = 0.;
        return;
    }

    template <typename Scalar>
    void lmpar2(const ColPivHouseholderQR<Matrix<Scalar, Dynamic, Dynamic>>& qr,
                const Matrix<Scalar, Dynamic, 1>& diag,
                const Matrix<Scalar, Dynamic, 1>& qtb,
                Scalar delta,
                Scalar& par,
                Matrix<Scalar, Dynamic, 1>& x)

    {
        using std::abs;
        using std::sqrt;
        typedef DenseIndex Index;

        /* Local variables */
        Index j;
        Scalar fp;
        Scalar parc, parl;
        Index iter;
        Scalar temp, paru;
        Scalar gnorm;
        Scalar dxnorm;

        /* Function Body */
        const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
        const Index n = qr.matrixQR().cols();
        eigen_assert(n == diag.size());
        eigen_assert(n == qtb.size());

        Matrix<Scalar, Dynamic, 1> wa1, wa2;

        /* compute and store in x the gauss-newton direction. if the */
        /* jacobian is rank-deficient, obtain a least squares solution. */

        //    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
        const Index rank = qr.rank();  // use a threshold
        wa1 = qtb;
        wa1.tail(n - rank).setZero();
        qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));

        x = qr.colsPermutation() * wa1;

        /* initialize the iteration counter. */
        /* evaluate the function at the origin, and test */
        /* for acceptance of the gauss-newton direction. */
        iter = 0;
        wa2 = diag.cwiseProduct(x);
        dxnorm = wa2.blueNorm();
        fp = dxnorm - delta;
        if (fp <= Scalar(0.1) * delta)
        {
            par = 0;
            return;
        }

        /* if the jacobian is not rank deficient, the newton */
        /* step provides a lower bound, parl, for the zero of */
        /* the function. otherwise set this bound to zero. */
        parl = 0.;
        if (rank == n)
        {
            wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2) / dxnorm;
            qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
            temp = wa1.blueNorm();
            parl = fp / delta / temp / temp;
        }

        /* calculate an upper bound, paru, for the zero of the function. */
        for (j = 0; j < n; ++j) wa1[j] = qr.matrixQR().col(j).head(j + 1).dot(qtb.head(j + 1)) / diag[qr.colsPermutation().indices()(j)];

        gnorm = wa1.stableNorm();
        paru = gnorm / delta;
        if (paru == 0.)
            paru = dwarf / (std::min)(delta, Scalar(0.1));

        /* if the input par lies outside of the interval (parl,paru), */
        /* set par to the closer endpoint. */
        par = (std::max)(par, parl);
        par = (std::min)(par, paru);
        if (par == 0.)
            par = gnorm / dxnorm;

        /* beginning of an iteration. */
        Matrix<Scalar, Dynamic, Dynamic> s = qr.matrixQR();
        while (true)
        {
            ++iter;

            /* evaluate the function at the current value of par. */
            if (par == 0.)
                par = (std::max)(dwarf, Scalar(.001) * paru); /* Computing MAX */
            wa1 = sqrt(par) * diag;

            Matrix<Scalar, Dynamic, 1> sdiag(n);
            qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);

            wa2 = diag.cwiseProduct(x);
            dxnorm = wa2.blueNorm();
            temp = fp;
            fp = dxnorm - delta;

            /* if the function is small enough, accept the current value */
            /* of par. also test for the exceptional cases where parl */
            /* is zero or the number of iterations has reached 10. */
            if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
                break;

            /* compute the newton correction. */
            wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2 / dxnorm);
            // we could almost use this here, but the diagonal is outside qr, in sdiag[]
            // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
            for (j = 0; j < n; ++j)
            {
                wa1[j] /= sdiag[j];
                temp = wa1[j];
                for (Index i = j + 1; i < n; ++i) wa1[i] -= s(i, j) * temp;
            }
            temp = wa1.blueNorm();
            parc = fp / delta / temp / temp;

            /* depending on the sign of the function, update parl or paru. */
            if (fp > 0.)
                parl = (std::max)(parl, par);
            if (fp < 0.)
                paru = (std::min)(paru, par);

            /* compute an improved estimate for par. */
            par = (std::max)(parl, par + parc);
        }
        if (iter == 0)
            par = 0.;
        return;
    }

}  // end namespace internal

}  // end namespace Eigen
